#include "ros_udp.hpp"
#include "tictoc.hpp"
#include <ros/ros.h>
#include <geometry_msgs/Pose.h>
#include <sensor_msgs/Image.h>
#include <cv_bridge/cv_bridge.h>  
#include <opencv2/opencv.hpp> 
#include "ros_udp_bridge/A8_attitude.h"
#include <thread>
#include <mutex>
#include <atomic> 

std::atomic<bool> stopFlag(false); // 全局原子布尔变量，用于设置停止标志 

bool ros_flag(false);
bool img_flag(false);
bool set_att_flag(false);



cv::Mat a8_image;

std::string url = "rtsp://192.168.144.25:8554/main.264"; 

//加锁器
std::mutex mtx;

void Get_Image_Func()
{
    cv::VideoCapture cap(url);
    

    double t = ros::Time::now().toSec();
    double last_t = ros::Time::now().toSec();
    double fps = 0;
    int cnt = 0;
    while(!stopFlag)
    {
        cv::Mat temp;
        cap >> temp;
        //cap.grab();
        //cap.retrieve(temp);
        if (temp.empty())//如果某帧为空则退出循环
        {
            std::cout<<"temp empty"<<std::endl;
            continue;
        }
        
        mtx.lock();
        a8_image = temp;
        mtx.unlock();
        
 
    
        
        cnt++;
        t = ros::Time::now().toSec();
        if(t-last_t>=1.0)
        {

            fps = cnt / (t-last_t);
            last_t = t;
            cnt=0;
            std::cout << " receive fps： " << fps << std::endl;
        }
   
        
    }
    cap.release();
    stopFlag = true;
    return ;
}



void ROS_Main(ros::NodeHandle nh)
{
    //初始化通讯
    int sockfd;
    //定义变量
    int recv_len;
    unsigned char recv_buf[RECV_BUUF_SIZE] = {0};
    if ((sockfd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) 
    {
        perror("socket");
        exit(1);
    }
    //设置等待时间
    struct timeval timeout;  
    timeout.tv_sec = 1;  
    timeout.tv_usec = 0;  
    if (setsockopt(sockfd, SOL_SOCKET, SO_RCVTIMEO, &timeout, sizeof(timeout)) < 0) {  
        std::cerr << "Failed to set socket option" << std::endl;  
        return ;  
    }
    //初始化云台控制函数
    UDP_ROS udp_sub(sockfd);
    if(udp_sub.Node_Init()) 
    {
        std::cout<<"udp Init Finished !!!"<<std::endl;
    }
    // 创建一个订阅者对象


    // 创建一个发布者对象
    ros::Publisher curr_att_pub = nh.advertise<ros_udp_bridge::A8_attitude>("/a8_node/curr_att", 100);
        //发布读取到的实时姿态信息
    ros::Publisher curr_img_pub = nh.advertise<sensor_msgs::Image>("/a8_node/curr_image",100);
        //发布读取到的实时图像信息
    // 设置发布频率
    ros::Rate loop_rate(10);
    double t = ros::Time::now().toSec();
    double last_t = ros::Time::now().toSec();
    double fps = 0;
    int cnt = 0;
    bool send1 = false;
    while(!stopFlag && ros::ok())
    {
        if(send1)
        {
            //发送姿态
            std::vector<double> set_pose(2);
            if(udp_sub.Change_Pose(set_pose)) 
            {
                std::cout<<"Change Pose Finish !!!"<<std::endl;
            }
        }
        else
        {
            ros_udp_bridge::A8_attitude curr_att;
            std::vector<double> curr_pose = udp_sub.Deserialize_msg(sockfd);
        }
        

        
        

        ros::spinOnce(); // 处理其他ROS事件  
        loop_rate.sleep(); // 按照设定的频率休眠一段时间  
    }
    // 关闭套接字
    close(sockfd);
    stopFlag = true;
    return ;
}
int main(int argc, char** argv)  
{ 
    //Ros节点
    ros::init(argc, argv, "ros_udp_bridge_node");
    ros::NodeHandle nh;

    //初始化RTSP线程
    //建立两个线程，一个只用于运行图像处理，另一个用于发布接收消息

    // std::thread Get_Img_thread(Get_Image_Func);
    std::thread Ros_thread(ROS_Main,nh);

    //运行线程

    // Get_Img_thread.join();
    Ros_thread.join();

    //关闭进程
    stopFlag = true;
    return 0;
}
